#include "GeoCuttableSurface.h"
#include "Normal.h"
#include "HLFeedbackShape.h"
#include "Coordinate.h"
#include "H3DHapticsDevice.h"
#include "GeoSurfaceCutter.h"
#include <math.h>

using namespace H3D;

H3DNodeDatabase GeoCuttableSurface::database("GeoCuttableSurface",
            &(newInstance<GeoCuttableSurface>),
            typeid(GeoCuttableSurface),
            &X3DComposedGeometryNode::database);

namespace GeoCuttableSurfaceInternals {
  FIELDDB_ELEMENT( GeoCuttableSurface, set_index, INPUT_ONLY );
  FIELDDB_ELEMENT( GeoCuttableSurface, index, INPUT_OUTPUT );
  FIELDDB_ELEMENT( GeoCuttableSurface, shouldCut, INPUT_OUTPUT );
}

    GeoCuttableSurface::GeoCuttableSurface(
        Inst< SFNode                  > _metadata,
        Inst< SFBound                 > _bound,
        Inst< DisplayList             > _displayList,
        Inst< SFColorNode             > _color,
        Inst< SFCoordinateNode        > _coord,
        Inst< SFNormalNode            > _normal,
        Inst< SFTextureCoordinateNode > _texCoord,
        Inst< SFBool                  > _ccw,
        Inst< SFBool                  > _colorPerVertex,
        Inst< SFBool                  > _normalPerVertex,
        Inst< SFBool                  > _solid,
        Inst< MFVertexAttributeNode   > _attrib,
		Inst< AutoNormal              > _autoNormal,
        Inst< MFInt32                 > _set_index,
        Inst< MFInt32                 > _index,
		Inst< SFBool				  > _shouldCut) :
            X3DComposedGeometryNode( _metadata, _bound, _displayList,
                                     _color, _coord, _normal, _texCoord,
                                     _ccw, _colorPerVertex, _normalPerVertex,
                                     _solid, _attrib ),
			autoNormal( _autoNormal ),   
			set_index( _set_index ),
			index( _index ),
			shouldCut(_shouldCut)
    {
        type_name = "GeoCuttableSurface";
        database.initFields( this );

		autoNormal->setName( "autoNormal" );
		autoNormal->setOwner( this );

		index->route( displayList );
		set_index->route( index, id );

		normalPerVertex->route( autoNormal );
		coord->route( autoNormal );
		index->route( autoNormal );
		ccw->route( autoNormal );

		coord->route( bound );

		db.theDB = new ShapeDatabase(this);
		coord->route(&db);
		index->route(&db);

		_wasCutting = false;
		_wasPressed = false;
		firstNode = NULL;
    }

	//void GeoCuttableSurface::AppendPoint(vector<int>& indices,
	//								  vector<int *>& currentIndices,
	//								  const vector<Vec3f>& points,
	//								  vector<Vec3f>& newPoints,
	//								  int index, int offset)
	//{
	//	if(currentIndices[index][offset] != -1)
	//	{
	//		indices.push_back(currentIndices[index][offset]);
	//	}
	//	else
	//	{
	//		// The point isn't already in the array, so add it
	//		newPoints.push_back(points[index]);
	//		currentIndices[index][offset] = newPoints.size() - 1;
	//		indices.push_back(currentIndices[index][offset]);
	//	}
	//}

  //  void GeoCuttableSurface::Cut(GeoCuttableSurface *& sideOne, GeoCuttableSurface *& sideTwo)
  //  {
		//sideOne = sideTwo = NULL;
		//Coordinate * theCoords = dynamic_cast<Coordinate *>(coord->getValue());
		//if(theCoords == NULL)
		//	return;

		//vector<Vec3f> points = theCoords->point->getValue();
		//vector<Vec3f> sideOnePoints;
		//vector<Vec3f> sideTwoPoints;
		//vector<int> sideOneIndices;
		//vector<int> sideTwoIndices;

		//const int SIDEONE = -1;
		//const int MIDDLE = 0;
		//const int SIDETWO = 1;
		//const float THRESHOLD = 0.001;
		//vector<int> sides(points.size(), 0);
		//vector<int*> currentIndex(points.size(), NULL);

		//for(int i = 0; i<points.size(); i++)
		//{
		//	currentIndex[i] = new int[2];
		//	currentIndex[i][0] = -1;
		//	currentIndex[i][1] = -1;

		//	if(points[i].x < -THRESHOLD)
		//	{
		//		cout<<"S1";
		//		sides[i] = SIDEONE;
		//		sideOnePoints.push_back(points[i]);
		//		currentIndex[i][0] = sideOnePoints.size() - 1;
		//	}
		//	else if(points[i].x > THRESHOLD)
		//	{
		//		cout<<"S2";
		//		sides[i] = SIDETWO;
		//		sideTwoPoints.push_back(points[i]);
		//		currentIndex[i][1] = sideTwoPoints.size() - 1;
		//	}
		//	else
		//	{
		//		cout<<"M";
		//		sides[i] = MIDDLE;
		//		sideOnePoints.push_back(points[i]);
		//		sideTwoPoints.push_back(points[i]);
		//		currentIndex[i][0] = sideOnePoints.size() - 1;
		//		currentIndex[i][1] = sideTwoPoints.size() - 1;
		//	}
		//}

		//cout<<endl;

		//for(int i = 0; i< (index->getValue().size()) / 3; i++)
		//{
		//	int ind0 = (index->getValue())[3 * i + 0];
		//	int ind1 = (index->getValue())[3 * i + 1];
		//	int ind2 = (index->getValue())[3 * i + 2];

		//	// For the moment, just move the points out of the middle
		//	if(sides[ind0] == MIDDLE)
		//	{
		//		sides[ind0] = SIDEONE;
		//	}
		//	if(sides[ind1] == MIDDLE)
		//	{
		//		sides[ind1] = SIDEONE;
		//	}
		//	if(sides[ind2] == MIDDLE)
		//	{
		//		sides[ind2] = SIDEONE;
		//	}

		//	if(sides[ind0] == sides[ind1] == sides[ind2])
		//	{
		//		if(sides[ind0] == SIDEONE)
		//		{
		//			cout<<"S1";
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind0, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind1, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind2, 0);
		//		}
		//		else if(sides[ind0] == SIDETWO)
		//		{
		//			cout<<"S2";
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind0, 1);
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind1, 1);
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind2, 1);
		//		}
		//		else
		//		{
		//			cout<<"M";
		//			// We're in the middle. For the moment, just add them all to side one.
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind0, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind1, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind2, 0);
		//		}
		//	}
		//	else
		//	{
		//		// They're on different sides. Use multiplication to figure out what's up,
		//		// then put the points in the appropriate list
		//		if(sides[ind0] * sides[ind1] * sides[ind2] < 0)
		//		{
		//			cout<<"P1";
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind0, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind1, 0);
		//			AppendPoint(sideOneIndices, currentIndex, points, sideOnePoints, ind2, 0);
		//		}
		//		else
		//		{
		//			cout<<"P2";
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind0, 1);
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind1, 1);
		//			AppendPoint(sideTwoIndices, currentIndex, points, sideTwoPoints, ind2, 1);
		//		}
		//	}
		//}

		//cout<<endl;

		//// Create the GeoCuttableSurfaces
		//sideOne = new GeoCuttableSurface();
		//sideTwo = new GeoCuttableSurface();

		//AutoRef<Coordinate> cSideOne(new Coordinate);
		//AutoRef<Coordinate> cSideTwo(new Coordinate);
		//cSideOne->point->setValue(sideOnePoints);
		//cSideTwo->point->setValue(sideTwoPoints);
		//sideOne->coord->setValue(cSideOne);
		//sideTwo->coord->setValue(cSideTwo);
		//sideOne->index->setValue(sideOneIndices);
		//sideTwo->index->setValue(sideTwoIndices);

		//for(int i = 0; i< points.size(); i++)
		//{
		//	delete[] currentIndex[i];
		//}
  //  }

	/// Traverse the scenegraph. A HLFeedbackShape is added for haptic
	/// rendering if haptics is enabled.
    void GeoCuttableSurface::traverseSG( TraverseInfo &ti )
    {
		db.getValue();

		if( ti.hapticsEnabled() && ti.getCurrentSurface() )
		{
			ti.addHapticShapeToAll( new HLFeedbackShape( this,
                                    ti.getCurrentSurface(),
                                    ti.getAccForwardMatrix(),
                                   index->size() ) );
		}

		H3DHapticsDevice* haptic = ti.getHapticsDevice(0);

		/*if(haptic->mainButton->getValue() && shouldCut->getValue() && this->isTouched->getValue().size() > 0 && this->isTouched->getValue()[0])
		{
			if(!_wasCutting)
			{
				_lastPosition = ti.getAccInverseMatrix() * 
                                      haptic->weightedProxyPosition->getValue();

				_wasCutting = true;
				_lastFace = db.theDB->GetClosestFace(_lastPosition);
				return;
			}

			cout<<"Attempting cut."<<endl;

			// Here's where we go mad with cutting
			// Right now the algorithm is a little stupid
			// It doesn't handle cases where we crossed more than one line in a motion.

			Vec3f curPos = ti.getAccInverseMatrix() * 
                                      haptic->weightedProxyPosition->getValue();

			int curFace = db.theDB->GetClosestFace(curPos);
			if(curFace == _lastFace)
			{
				cout<<"Cut avoided with face "<<curFace<<endl;
				return;
			}

		}
		else
		{
			_wasCutting = false;
		}*/

		if(shouldCut->getValue())
		{
			if(haptic->mainButton->getValue())
			{
				_wasPressed = true;
			}
			else if(_wasPressed)
			{
				_wasPressed = false;
				if(_wasCutting)
				{
					// We've got the second point. Do the cut, man.
					_wasCutting = false;
					cout<<"Cut time."<<endl;
					//db.theDB->DeleteEdge(0);
					//index->setValue(db.theDB->GetIndices());
					Vec3f curPos = ti.getAccInverseMatrix() * 
                                      haptic->weightedProxyPosition->getValue();
					if(curPos != _lastPosition)
					{
						/*
						X3DCoordinateNode * tcd = static_cast<X3DCoordinateNode *>(this->coord->getValue());
						Coordinate * ncoord = dynamic_cast<Coordinate *>(tcd);
						const vector<Vec3f> & points = ncoord->point->getValue();

						float s;
						Vec3f n;
						n.y = -(_lastPosition.x - curPos.x) / (_lastPosition.y - curPos.y);
						n.x = 1.0f;
						n.z = 0.0f;
						n.normalizeSafe();
						vector<GeoCuttableSurface::ShapeDatabase::Edge *> deleteEdges;

						set<GeoCuttableSurface::ShapeDatabase::Edge *>::iterator i;
						for( i = db.theDB->edges.begin(); i != db.theDB->edges.end(); i++)
						{
							s = (-n).dotProduct(_lastPosition - points[(*i)->endpoints[0]->index]);
							s /= n.dotProduct(points[(*i)->endpoints[1]->index] - points[(*i)->endpoints[0]->index]);
							if(0.0f<=s && s<=1.0f)
							{
								cout<<"Edge found."<<endl;
								deleteEdges.push_back(*i);
							}
						}

						cout<<"Deleting "<<deleteEdges.size()<<" edges."<<endl;
						cout.flush();
						for(int i = 0; i<deleteEdges.size(); i++)
						{
							db.theDB->DeleteEdge(deleteEdges[i]);
						}
						*/

						SpringCutter cutter;
						X3DCoordinateNode * tcd = static_cast<X3DCoordinateNode *>(this->coord->getValue());
						Coordinate * ncoord = dynamic_cast<Coordinate *>(tcd);
						vector<Vec3f> points = ncoord->point->getValue();
						GeoCuttableSurface::ShapeDatabase::Point * pointList[2];

						//Mike Oren: Get position of touched points.
						//int i1, i2;
						//i1 = i2 = 0;
						//Vec3f point1;
						//Vec3f point2;
						//point1 = Vec3f(99.0,99.0,99.0);
						//point2 = Vec3f(99.0,99.0,99.0);

						//for(int i = 0; i<points.size(); i++){
						//	if (points[i].x == _lastPosition.x && points[i].y == _lastPosition.y){
						//		i1 = i;
						//		point1 = points[i];
						//		break;
						//	}
						//	if(_lastPosition.x >= float(0)){
						//		if(_lastPosition.y>=float(0)){
						//			if (abs(points[i].x - _lastPosition.x) < abs(point1.x + _lastPosition.x) && abs(points[i].y - _lastPosition.y) < abs(point1.y - _lastPosition.y)){
						//				i1 = i;
						//				cout<<"blah\n";
						//				point1 = points[i];
						//			}
						//		}
						//		else{
						//			if (abs(points[i].x - _lastPosition.x) < abs(point1.x - _lastPosition.x) && abs(points[i].y + _lastPosition.y) < abs(point1.y + _lastPosition.y)){
						//				i1 = i;
						//				cout<<"blah\n";
						//				point1 = points[i];
						//			}
						//		}
						//	}
						//	else{
						//		if(_lastPosition.y>=float(0)){
						//			if (abs(points[i].x + _lastPosition.x) < abs(point1.x + _lastPosition.x) && abs(points[i].y - _lastPosition.y) < abs(point1.y - _lastPosition.y)){
						//				i1 = i;
						//				cout<<"blah\n";
						//				point1 = points[i];
						//			}
						//		}
						//		else{
						//			if (abs(points[i].x + _lastPosition.x) < abs(point1.x + _lastPosition.x) && abs(points[i].y + _lastPosition.y) < abs(point1.y + _lastPosition.y)){
						//				i1 = i;
						//				cout<<"blah\n";
						//				point1 = points[i];
						//			}
						//		}
						//	}
						//}
						//cout<<"---------"<<endl;
						//for(int i = 0; i<points.size(); i++){
						//	if (float(points[i].x) == float(curPos.x) && float(points[i].y) == float(curPos.y)){
						//		i2 = i;
						//		cout<<"blah\n";
						//		point2 = points[i];
						//		break;
						//	}
						//	if(curPos.x >= float(0)){
						//		if(curPos.y>=float(0)){
						//			if (abs(points[i].x - curPos.x) < abs(point2.x - curPos.x) && abs(points[i].y - curPos.y) < abs(point2.y - curPos.y)){
						//				i2 = i;
						//				cout<<"blah\n";
						//				point2 = points[i];
						//			}
						//		}
						//		else{
						//			if (abs(points[i].x - curPos.x) < abs(point2.x - curPos.x) && abs(points[i].y + curPos.y) < abs(point2.y + curPos.y)){
						//				i2 = i;
						//				cout<<"blah\n";
						//				point2 = points[i];
						//			}
						//		}
						//	}
						//	else{
						//		if(curPos.y>=float(0)){
						//			if (abs(points[i].x + curPos.x) < abs(point2.x + curPos.x) && abs(points[i].y - curPos.y) < abs(point2.y - curPos.y)){
						//				i2 = i;
						//				cout<<"blah\n";
						//				point2 = points[i];
						//			}
						//		}
						//		else{
						//			if (abs(points[i].x + curPos.x) < abs(point2.x + curPos.x) && abs(points[i].y + curPos.y) < abs(point2.y + curPos.y)){
						//				i2 = i;
						//				cout<<"blah\n";
						//				point2 = points[i];
						//			}
						//		}
						//	}
						//}

						pointList[0] = *db.theDB->points.begin();
						pointList[1] = *(--db.theDB->points.end());

						//////Mike Oren: Find the corner points...
						////GeoCuttableSurface::ShapeDatabase::Point* pnt = *db.theDB->points.begin();
						////while(pnt != *(--db.theDB->points.end())){
						////	if(pnt->index == NULL)
						////		break;
						////	if(pnt->index >= 0){
						////		if(pnt->index < points.size() - 1){
						////			if(points[pnt->index].x <= points[pointList[0]->index].x && points[pnt->index].y >= points[pointList[0]->index].y){
						////				pointList[0] = pnt;
						////			}
						////			if(points[pnt->index].x >= points[pointList[1]->index].x && points[pnt->index].y<= points[pointList[1]->index].y){
						////				pointList[1] = pnt;
						////			}
						////		}
						////	}
						////	++pnt;
						////}

						//Vec3f point3;
						//Vec3f point4;
						//point3 = Vec3f(99.0,99.0,99.0);
						//point4 = Vec3f(99.0,99.0,99.0);

						//int counter = 0;

						//GeoCuttableSurface::ShapeDatabase::Point* pnt = *db.theDB->points.begin();
						//while(counter < db.theDB->points.size()){
						//	cout<<pnt->index<<endl;
						//	if(pnt->index < points.size()){
						//		if(pnt->index == i1){
						//			pointList[0] = pnt;
						//		}
						//		if(pnt->index == i2){
						//			pointList[1] = pnt;
						//		}
						//	}
						//	//if(pnt->index >= 0){
						//	//	if(pnt->index < points.size() - 1){
						//	//		/*if(points[pnt->index].x <= points[pointList[0]->index].x && points[pnt->index].y >= points[pointList[0]->index].y){
						//	//			pointList[0] = pnt;
						//	//		}
						//	//		if(points[pnt->index].x >= points[pointList[1]->index].x && points[pnt->index].y<= points[pointList[1]->index].y){
						//	//			pointList[1] = pnt;
						//	//		}*/
						//	//		if(points[pnt->index] == point1){
						//	//			pointList[0] = pnt;
						//	//			point3 = points[pnt->index];
						//	//		}
						//	//		else if(abs(points[pnt->index].x - point1[0]) < abs(point3[0] - point1[0]) && abs(points[pnt->index].y - point1[1]) < abs(point3[1] - point1[1])){
						//	//			pointList[0] = pnt;
						//	//			point3 = points[pnt->index];
						//	//		}
						//	//		if(points[pnt->index] == point2)
						//	//			pointList[1] = pnt;
						//	//		else if(abs(points[pnt->index].x - point2[0]) < abs(point4[0] - point2[0]) && abs(points[pnt->index].y - point2[1]) < abs(point4[1] - point2[1])){
						//	//			pointList[1] = pnt;
						//	//			point4 = points[pnt->index];
						//	//		}
						//	//	}
						//	//}
						//	++pnt;
						//	counter++;
						//}

						/*cout<<pointList[0]<<" | "<<pointList[1]<<" * "<<point1<<" | "<<point2<<endl;
						cout<<curPos<<" | "<<_lastPosition<<endl;	*/
						
						//if(pointList[0]->index <= points.size() && pointList[1]->index <= points.size()){
						cutter.Cut(*db.theDB, points, pointList, 2);

						ncoord->point->setValue(points);
						index->setValue( db.theDB->GetIndices());
						//}
						/*else
						{
							cout<<"No cut: Point not found."<<endl;
							cout<<i1<<" "<<i2<<endl;
						}*/
					}
					else
					{
						cout<<"No cut: Positions the same."<<endl;
					}
				}
				else
				{
					_wasCutting = true;
					_lastPosition = ti.getAccInverseMatrix() * 
                                      haptic->weightedProxyPosition->getValue();
					
					//TODO JDS Get point, call subdivide, this is our first "marker"
					// Then in the other code call "Cut" using this as a marker

				}
			}
		}
	}

	/// Renders the IndexedTriangleSet with OpenGL.
    void GeoCuttableSurface::render()
    {
		X3DCoordinateNode *coordinate_node = coord->getValue();
		X3DTextureCoordinateNode *tex_coord_node = texCoord->getValue();
		TextureCoordinateGenerator *tex_coord_gen = 
			dynamic_cast< TextureCoordinateGenerator * >( tex_coord_node );
		X3DColorNode *color_node = color->getValue();
		X3DNormalNode *normal_node = normal->getValue();

		if( !normal_node )
		{
			normal_node = autoNormal->getValue();
		}

		bool tex_coords_per_vertex = tex_coord_node && !tex_coord_gen;
		const vector< int > indices = db.theDB->GetIndices();
		cout<<"Indices gotten: "<<indices.size()<<endl;
		cout.flush();
 
		glShadeModel( GL_SMOOTH ); 
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

		if( coordinate_node )
		{
			// enable backface culling if solid is true
			if( solid->getValue() )
			{
				glEnable( GL_CULL_FACE );
				glCullFace( GL_BACK );
			}
			else
			{
				glDisable( GL_CULL_FACE );
			}

		if( !tex_coords_per_vertex )
		{
			startTexGen( tex_coord_gen );
		}
		else if( coordinate_node->nrAvailableCoords() > 
               tex_coord_node->nrAvailableTexCoords() )
		{
			stringstream s;
			s << "Must contain at least as many elements as coord (" 
			  << coordinate_node->nrAvailableCoords() << ") in \"" 
              << getName() << "\" node. ";
			throw NotEnoughTextureCoordinates( tex_coord_node->nrAvailableTexCoords(),
                                               s.str(), H3D_FULL_LOCATION );
		}

		// if we have a color node we use the color from that instead
		// of the previously installed Material node.
		if( color_node )
		{
			// Make sure we have enough colors      
			if( coordinate_node->nrAvailableCoords() > 
                color_node->nrAvailableColors() )
			{
				stringstream s;
				s << "Must contain at least as many elements as coord (" 
				  << coordinate_node->nrAvailableCoords() << ") in \"" 
				  << getName() << "\" node. ";
				throw NotEnoughColors( color_node->nrAvailableColors(),
                                       s.str(), H3D_FULL_LOCATION );
			}

			glEnable( GL_COLOR_MATERIAL );
		}

		GLhandleARB shader_program = 0;
		// Set the attribute index to use for all vertex attributes
		if( GLEW_ARB_shader_objects && GLEW_ARB_vertex_shader )
		{
			shader_program = glGetHandleARB( GL_PROGRAM_OBJECT_ARB );
			if( shader_program )
			{
				for( unsigned int i = 0; i < attrib->size(); i++ )
				{
					X3DVertexAttributeNode *attr = attrib->getValueByIndex( i );
					if( attr )
					{
						GLint loc = 
							glGetAttribLocationARB( shader_program, 
													attr->name->getValue().c_str()); 
						attr->setAttribIndex( loc );
					}
				}
			}
		}

		unsigned int nr_triangles = indices.size() / 3;

		if( normalPerVertex->getValue() )
		{
			// if normal per vertex we can use arrays to render the geometry
			// they all use the same indices.
			coordinate_node->renderArray();
			normal_node->renderArray();
			if( color_node ) color_node->renderArray();
			if( tex_coords_per_vertex ) renderTexCoordArray( tex_coord_node );
			for( unsigned int attrib_index = 0;
				 attrib_index < attrib->size(); attrib_index++ )
			{
				X3DVertexAttributeNode *attr = 
					attrib->getValueByIndex( attrib_index );
				if( attr ) attr->renderArray();
			}
      
			glDrawElements( GL_TRIANGLES, 
							3*nr_triangles, 
							GL_UNSIGNED_INT,
							&(*(indices.begin()) ) );
      
			coordinate_node->disableArray();
			normal_node->disableArray();
			if( color_node ) color_node->disableArray();
			if( tex_coords_per_vertex ) disableTexCoordArray( tex_coord_node );
			for( unsigned int attrib_index = 0;
				 attrib_index < attrib->size(); attrib_index++ )
			{
				X3DVertexAttributeNode *attr = 
				attrib->getValueByIndex( attrib_index );
				if( attr ) attr->disableArray();
			}      
		}
		else
		{
			// we cannot use arrays to render when normal_per_vertex is false
			// since the indexing into the normals are different from the rest.
			glBegin( GL_TRIANGLES );
			for( unsigned int i = 0; i < nr_triangles; i++ )
			{
				normal_node->render( i );

				unsigned int v = i * 3;
				// vertex 1
				if( color_node ) color_node->render( indices[v] );
				if( tex_coords_per_vertex ) renderTexCoord( indices[v],
                                                            tex_coord_node );
				for( unsigned int attrib_index = 0;
					 attrib_index < attrib->size(); attrib_index++ )
				{
					X3DVertexAttributeNode *attr = 
					attrib->getValueByIndex( attrib_index );
					if( attr ) attr->render( indices[v] );
				}
			coordinate_node->render( indices[v] );

	        // vertex 2
			if( color_node ) color_node->render( indices[v+1] );
			if( tex_coords_per_vertex ) renderTexCoord( indices[v+1],
				                                        tex_coord_node );
			for( unsigned int attrib_index = 0;
				 attrib_index < attrib->size(); attrib_index++ )
			{
				X3DVertexAttributeNode *attr = 
					attrib->getValueByIndex( attrib_index );
				if( attr ) attr->render( indices[v+1] );
			}
			coordinate_node->render( indices[v+1] );

			// vertex 3
			if( color_node ) color_node->render( indices[v+2] );
			if( tex_coords_per_vertex ) renderTexCoord( indices[v+2],
														tex_coord_node );
			for( unsigned int attrib_index = 0;
				 attrib_index < attrib->size(); attrib_index++ )
			{
				X3DVertexAttributeNode *attr = 
					attrib->getValueByIndex( attrib_index );
				if( attr ) attr->render( indices[v+2] );
			}
			coordinate_node->render( indices[v+2] );
		}
		glEnd();
	}
    // disable texture coordinate generation.
    if( !tex_coords_per_vertex )
	{
      stopTexGen( tex_coord_gen );
    }

    if ( color_node )
	{
      glDisable( GL_COLOR_MATERIAL );
    } 
  } 
}

    void GeoCuttableSurface::AutoNormal::update()
    {
        bool normals_per_vertex =
            static_cast< SFBool * >( routes_in[0] )->getValue();
        X3DCoordinateNode *coord =
            static_cast< X3DCoordinateNode * >( static_cast< SFCoordinateNode * >
                                                ( routes_in[1] )->getValue() );
        const vector<int> &index =
            static_cast< MFInt32 * >( routes_in[2] )->getValue();
        bool ccw = static_cast< SFBool * >( routes_in[3] )->getValue();

        if ( normals_per_vertex )
            value = generateNormalsPerVertex( coord, index, ccw );
        else
            value = generateNormalsPerFace( coord, index, ccw );
    }

    X3DNormalNode *GeoCuttableSurface::AutoNormal::generateNormalsPerVertex(
        X3DCoordinateNode *coord,
        const vector< int > &index,
        bool ccw )
    {
        Normal *normal = new Normal;
        if ( coord )
        {
            vector< Vec3f > normals( coord->nrAvailableCoords(),
                                     Vec3f( 0, 0, 0 ) );
            for ( unsigned int j = 0; j < index.size(); j+=3 )
            {
                Vec3f norm, A, B, C, AB, BC;
                // make sure we have a valid face. If not use a dummy normal.
                if ( j+2 >= index.size() )
                {
                    norm =  Vec3f( 1, 0, 0 );
                }
                else
                {
                    // calculate a normal
                    A = coord->getCoord( index[ j ] );
                    B = coord->getCoord( index[ j+1 ] );
                    C = coord->getCoord( index[ j+2 ] );

                    AB = B - A;
                    BC = C - B;

                    norm = AB % BC;
                }
                norm.normalizeSafe();
                if ( !ccw )
                    norm = -norm;
                normals[index[ j ]] += norm;
                normals[index[ j+1 ]] += norm;
                normals[index[ j+2 ]] += norm;
            }

            for ( vector<Vec3f>::iterator i = normals.begin();
                    i != normals.end();
                    i++ )
            {
                (*i).normalizeSafe();
            }

            normal->vector->setValue( normals );
        }
        return normal;
    }

    X3DNormalNode *GeoCuttableSurface::AutoNormal::generateNormalsPerFace(
        X3DCoordinateNode *coord,
        const vector< int > &index,
        bool ccw )
    {
        Normal *normal = new Normal;
        if ( coord )
        {
            vector< Vec3f > normals;
            for ( size_t j = 0; j < index.size(); j+=3 )
            {
                Vec3f norm, A, B, C, AB, BC;
                // make sure we have a valid face. If not use a dummy normal.
                if ( j+2 >= index.size() )
                {
                    norm =  Vec3f( 1, 0, 0 );
                }
                else
                {
                    // calculate a normal for the triangle
                    A = coord->getCoord( index[ j ] );
                    B = coord->getCoord( index[ j + 1 ] );
                    C = coord->getCoord( index[ j + 2 ] );

                    AB = B - A;
                    BC = C - B;

                    norm = AB % BC;
                }

                try
                {
                    norm.normalize();
                }
                catch ( const ArithmeticTypes::Vec3f::Vec3fNormalizeError & )
                {
                    norm = Vec3f( 1, 0, 0 );
                }

                if ( !ccw )
                    norm = -norm;

                normals.push_back( norm );
            }
            normal->vector->setValue( normals );
        }
        return normal;
    }

	GeoCuttableSurface::ShapeDatabase::Face * GeoCuttableSurface::ShapeDatabase::AddFace(ShapeDatabase::Point * pt0, ShapeDatabase::Point * pt1, ShapeDatabase::Point * pt2)
	{
		Edge * edge0 = AddEdge(pt0, pt1);
		Edge * edge1 = AddEdge(pt1, pt2);
		Edge * edge2 = AddEdge(pt2, pt0);

		Face * f = new Face;
		f->edges[0] = edge0;
		f->edges[1] = edge1;
		f->edges[2] = edge2;
		f->points[0] = pt0;
		f->points[1] = pt1;
		f->points[2] = pt2;

		pt0->faces.push_back(f);
		pt1->faces.push_back(f);
		pt2->faces.push_back(f);
		
		faces.insert(faces.begin(), f);

		// Fill in edge0's faces
		if(edge0->faces[0] == NULL)
		{
			edge0->faces[0] = f;
		}
		else if(edge0->faces[1] == NULL)
		{
			edge0->faces[1] = f;
		}
		else
		{
			cout<<"Error: edge 0 full"<<endl;
		}

		// Fill in edge1's faces
		if(edge1->faces[0] == NULL)
		{
			edge1->faces[0] = f;
		}
		else if (edge1->faces[1] == NULL)
		{
			edge1->faces[1] = f;
		}
		else
		{
			cout<<"Error: edge 1 full"<<endl;
		}

		// Fill in edge2's faces
		if(edge2->faces[0] == NULL)
		{
			edge2->faces[0] = f;
		}
		else if(edge2->faces[1] == NULL)
		{
			edge2->faces[1] = f;
		}
		else
		{
			cout<<"Error: edge 2 full"<<endl;
		}

		return f;
	}

	GeoCuttableSurface::ShapeDatabase::Edge * GeoCuttableSurface::ShapeDatabase::AddEdge(Point * pt0, Point * pt1)
	{
		if(pt0 == pt1)
		{
			cout<<"Something bad is going on; pt0 == pt1"<<endl;
			return NULL;
		}

		// Check if the points are already connected
		// If they are, return that edge
		// else, create the edge and return the index
		//
		// TODO: Nested loops could be bad. Other algorithms for finding edge?
		for(int i = 0; i<pt0->edges.size(); i++)
		{
			for(int j = 0; j<pt1->edges.size(); j++)
			{
				if(pt0->edges[i] == pt1->edges[j])
				{
					return pt0->edges[i];
				}
			}
		}

		Edge * newEdge = new Edge;
		newEdge->endpoints[0] = pt0;
		newEdge->endpoints[1] = pt1;
		//newEdge->lengthSq = CalcLengthSquared(pt0, pt1);
		edges.insert(edges.begin(), newEdge);
		pt0->edges.push_back(newEdge);
		pt1->edges.push_back(newEdge);
		return newEdge;
	}

	void GeoCuttableSurface::ShapeDatabase::DeleteFace(Face * f, bool deleteEdges)
	{
		cout<<"DeleteFace "<<f<<endl;
		if(f->deleting)
			return;

		f->deleting = true;

		// Remove face from each point's list of faces
		for(int i = 0; i<3; i++)
		{
			if(f->points[i] != NULL)
			{
				int faceIndex = -1;
				for(int j = 0; j<f->points[i]->faces.size(); j++)
				{
					if(f->points[i]->faces[j] == f)
					{
						faceIndex = j;
						break;
					}
				}

				if(faceIndex != -1)
				{
					if(faceIndex != f->points[i]->faces.size() - 1)
					{
						f->points[i]->faces[faceIndex] = f->points[i]->faces[f->points[i]->faces.size() - 1];
					}

					f->points[i]->faces.pop_back();
				}
			}
		}

		for(int i = 0; i<3; i++)
		{
			if(f->edges[i]->faces[0] == f)
			{
				f->edges[i]->faces[0] = NULL;
			}
			else if(f->edges[i]->faces[1] == f)
			{
				f->edges[i]->faces[1] = NULL;
			}
			else
			{
				cout<<"Error in edges found: no edge matches face"<<endl;
			}
			
			if(deleteEdges && f->edges[i]->faces[0] == f->edges[i]->faces[1] && f->edges[i]->faces[0] == NULL)
			{
				DeleteEdge(f->edges[i]);
			}
		}

		faces.erase(f);
		delete f;
	}

	void GeoCuttableSurface::ShapeDatabase::DeleteEdge(Edge * e)
	{
		cout<<"Deleting edge "<<e<<endl;
		cout.flush();

		if(e->deleting)
			return;

		e->deleting = true;

		if(e->endpoints[0]->edges.size() <= 1)
		{
			DeletePoint(e->endpoints[0]);
		}
		else
		{
			int deleteOffset = -1;
			for(int i = 0; i<e->endpoints[0]->edges.size(); i++)
			{
				if(e->endpoints[0]->edges[i] == e)
				{
					deleteOffset = i;
					break;
				}
			}

			if(deleteOffset != -1)
			{
				int lastIndex = e->endpoints[0]->edges.size() - 1;
				if(deleteOffset != lastIndex)
				{
					e->endpoints[0]->edges[deleteOffset] = e->endpoints[0]->edges[lastIndex];
				}

				e->endpoints[0]->edges.pop_back();
			}
		}

		if(e->endpoints[1]->edges.size() <= 1)
		{
			DeletePoint(e->endpoints[1]);
		}
		else
		{
			int deleteOffset = -1;
			for(int i = 0; i<e->endpoints[1]->edges.size(); i++)
			{
				if(e->endpoints[1]->edges[i] == e)
				{
					deleteOffset = i;
					break;
				}
			}

			if(deleteOffset != -1)
			{
				int lastIndex = e->endpoints[1]->edges.size() - 1;
				if(deleteOffset != lastIndex)
				{
					e->endpoints[1]->edges[deleteOffset] = e->endpoints[1]->edges[lastIndex];
				}

				e->endpoints[1]->edges.pop_back();
			}
		}

		if(e->faces[0] != NULL)
		{
			DeleteFace(e->faces[0]);
		}

		if(e->faces[1] != NULL)
		{
			DeleteFace(e->faces[1]);
		}

		edges.erase(e);
		delete e;
	}

	void GeoCuttableSurface::ShapeDatabase::DeletePoint(Point * p)
	{
		cout<<"DeletePoint "<<p<<endl;
		cout.flush();
		if(p->deleting)
			return;

		p->deleting = true;

		for(int i = 0; i<p->edges.size(); i++)
		{
			DeleteEdge(p->edges[i]);
		}
		
		points.erase(p);
		delete p;
	}

	// first, we use this function to go around the cut and label all the
	// adjacent nodes as inside, outside, or on the plane
	// We must do this as a separate, preprocessing step because if we were
	// to do the cuts as we go, we could cut something, only to later find that
	// we wish we hadn't...
	void GeoCuttableSurface::ShapeDatabase::Point::ColorMeshForSplitAtNode(GeoCuttableSurface::ShapeDatabase::Point* other_node, ShapeDatabase & db, vector<Vec3f> & coords)
	{   
		bool debug = true;
		// a handy pointer to us to make semantics easier
		Point* us = this;
		//us->setPartId(1);
	    
	#define DUMMY_FACE
	#ifdef DUMMY_FACE
		// create a dummy face
		/*Point  *n1,  *n2,  *n3;
		n1->index = index; n2.index = index + us->getNormal(); n3.index = other_node->index;
		Edge  *e1,  *e2,  *e3;
		e1.init(NULL, NULL, &n1, &n2);
		e2.init(NULL, NULL, &n2, &n3);
		e3.init(NULL, NULL, &n3, &n1);*/
		//Face f;
		/*f.init(NULL, NULL, NULL, &n1, &n2, &n3, &e1, &e2, &e3);
		f.computenormal();*/
	#else
		Object* obj = nodearray_p->getObject();
		Point3D normal = us->getNormal();
		int i1 = nodearray_p->addNode(p-4*normal);
		int i2 = nodearray_p->addNode(p + 4*us->getNormal());
		int i3 = nodearray_p->addNode(other_node->p-4*normal);
		FaceArray* facearray_p = obj->getFaceArray();
		int face_index = facearray_p->addFace(i1,i2,i3);
		Face* f = facearray_p->getFace(face_index);
		f->computenormal();
	#endif
	    
		// loop over all edges to get adjacent nodes
		cerr << "moving edges:\n";
		for (int i=0; i<edges.size(); i++) {
			Edge* cur_edge = edges[i];
			Point* adj_node;
			if(cur_edge->endpoints[0] == this)
				adj_node = cur_edge->endpoints[1];
			else
				adj_node = cur_edge->endpoints[0];
	        
			// internal sanity checks
			/*if (cur_edge->getIndex() == -1) {
				cerr << "WE'VE GONE NUTS WITH EDGELINK " << i << endl;
				continue;
			}*/
	        
			// look at signed distance to that face to determine which side 
			// of our cutting plane we're on
			// double dist = f.PlaneDistanceToPoint(adj_node->p);

			//JDS Instead we'll do this calculation ourself, w/o a dummy face.
			// TODO JDS Check if z really is up and if we'd be better off actually calculating normals
			Vec3f a = coords[this->index];
			Vec3f b = coords[this->index] + Vec3f(0.0, 0.0, 1.0);
			Vec3f c = coords[other_node->index];

			Vec3f u = b - a, v = c - a;

			Vec3f normal(u.y*v.z - u.z*v.y, u.z*v.x - u.x*v.z, u.x*v.y - u.y*v.x);
			normal.normalizeSafe();

			double plane_constant = normal.dotProduct(coords[this->index]);
			double pt_constant = normal.dotProduct(coords[adj_node->index]);
			double dist = (pt_constant - plane_constant);
	        
			// if distance is negative
			/*if (debug)
				cerr << "Edge[" << i << "/" << edges.size() << "] (" 
				<< cur_edge->getIndex() << ") " << *cur_edge << " ";*/
			if (dist == 0.0) {
				if (debug)
					cerr << "is on the plane = outside\n";
				//adj_node->setPartId(2);	// debugging
				adj_node->setPartId(3);
			}
			else if (dist > 0.0) {
				if (debug)
					cerr << "is on the outside\n";
				adj_node->setPartId(3);
			}
			else {    // is on inside
				// if we've already called it outside, don't second guess
				if (adj_node->getPartId() == 3) {
					if (debug)
						cerr << "node " << adj_node 
						<< " WAS ALREADY COLORED OUTSIDE\n";
					continue;
				}
	            
				if (debug)
					cerr << "is on the inside\n";
				adj_node->setPartId(4);
			}
		} // for i over edges
	}

	// Splits mesh given coloring produced by ColorMeshForSplitAtNode
	GeoCuttableSurface::ShapeDatabase::Point* GeoCuttableSurface::ShapeDatabase::Point::SplitColoredMeshAtNode(GeoCuttableSurface::ShapeDatabase::Point* other_node, ShapeDatabase & db, vector<Vec3f> & coords)
	{
		//TODO JDS Remove or make this false
		bool debug = true;
		if (debug)
			cerr << "Node[" << this << "]::SplitColoredMeshAtNode(" 
			<< other_node << ")\n";
	    
		// a handy pointer to us to make semantics easier
		Point* us = this;
	    
		// new node if we need it
		Point* new_us = NULL;
	    
		{ // loop over all edges to get adjacent nodes
	        
			// move all the edges
			if (debug) cerr << "moving edges:\n";
			for (int i=0; i<edges.size(); i++) {
				Edge* cur_edge = edges[i];
				Point* adj_node;
				if(cur_edge->endpoints[0] == this)
					adj_node = cur_edge->endpoints[1];
				else
					adj_node = cur_edge->endpoints[0];
	            
				// internal sanity checks
				/*if (cur_edge->getIndex() == -1) {
					cerr << "WE'VE GONE NUTS WITH EDGELINK " << i << endl;
					continue;
				}*/
	            
				// if distance is negative
				if (debug)
					cerr << "Edge[" << i << "/" << edges.size() << "] (" 
					<< cur_edge << ") ";
				if (adj_node->getPartId() == 2) {	// is on the plane
					if (debug) cerr << "is on the plane\n";
				}
				else if (adj_node->getPartId() == 3) {	// is on the good side
					if (debug) cerr << "is on the good side\n";
				}
				else {    // is on bad side- colored 4
					if (debug) cerr << "is on the bad side\n";
	                
					// if new_us doesn't exist, create it (at our location)
					if (!new_us) {
						new_us = new Point;

						//TODO JDS We NEED to add a new point to coords rather than using the same index
						new_us->index = index;
						db.points.insert(new_us);
						
						if (debug) cerr << "New_us at " << new_us->index << endl;
					}
	                
					// fixup edges that use us to use new_us
					if (debug)
						cerr << "fixing edge#" << cur_edge 
						<< " to use node at " << new_us->index 
						<< " instead of " << us->index << endl;
					if(cur_edge->endpoints[0] == us)
						cur_edge->endpoints[0] = new_us;
					else if(cur_edge->endpoints[1] == us)
						cur_edge->endpoints[1] = new_us;

					new_us->edges.push_back(cur_edge);
					// would like to us->deleteEdgeLink(cur_edge); here, but can't
					//    because it would mess up the array we're iterating over!
					//    - mark it as null and delete later...
					edges[i] = NULL;
				} // if dist
			}  // for edges
	        
			{ // delete any null edges
				for (int i=0; i<edges.size(); i++) {
					Edge* e = edges[i];
					if (e == NULL)
					{
						edges[i] = edges[edges.size() - 1];
						edges.pop_back();
						i--; // need to bump the counter down because of Delete
					}
				}
			}
		}

		if(debug)
		{
			cerr<<"new_us: "<<new_us<<endl;
		}
	    
		// fixup all faces if we had to split off an edge
		if (new_us) { 
			// make a local copy of our face links because we modify our own
			//    list of face links as we go
			vector<Face*> local_face_links(10);
			{ for (int i=0; i<faces.size(); i++) 
				local_face_links.push_back(faces[i]);
			}
	        
	        
			// fix the faces
			if (debug) cerr << "fixing faces\n";
			for (int i=0; i<local_face_links.size(); i++) {
				Face* cur_face = local_face_links[i];
				if (debug) 
					cerr << "doing face[" << i << "] (" << cur_face
					<< "): " << endl;

				if(!cur_face){//Mike Oren: sanity check
					cerr<<"SANITY: cur_face doesn't exist!!!!!\n";
					continue;
				}
	            
				// look at the edges we have and see where they go
				Edge *us_edge = NULL, *new_us_edge = NULL;
				for (int j=0; j<3; j++) {
					Edge* e = cur_face->edges[j];
					if (!e) { // sanity check
						cerr << "SANITY: cur_face didn't have a valid edge!!!!!\n";
						continue;    
					}
					if ((e->endpoints[0] == us) || (e->endpoints[1] == us)) 
						us_edge = e;
					if ((e->endpoints[0] == new_us) || (e->endpoints[1] == new_us)) 
						new_us_edge = e;
				}
	            
				// if all edges are only on the bad side, move edge to new_us and roll
				if (!us_edge && new_us_edge) {
					if (debug)
						cerr << "face is totally on bad side- "
						<< "switching links to new_us\n";
	                
					// switch the face link
					for(int i = 0; i<3; i++)
					{
						if(cur_face->points[i] == us)
							cur_face->points[i] = new_us;
					}
	                
					// fix the node links
					new_us->faces.push_back(cur_face);
					int faceIndex = -1;
					for(int i = 0; i<faces.size(); i++)
					{
						if(faces[i] == cur_face)
						{
							faceIndex = i;
							break;
						}
					}

					if(faceIndex != -1)
					{
						faces[faceIndex] = faces[faces.size() - 1];
						faces.pop_back();
					}  
					continue;
				}
	            
				// if all edges are only on the good side, keep rolling
				if (us_edge && !new_us_edge ) {
					if (debug) 
						cerr << "face is totally on good side- nothing to do\n";
					continue;
				}
	            
				// otherwise, is a split face- one edge on good side and one on bad
				if (debug) cerr << "face is a split face! ";
	            
				// if is a split triangle, with one edge in each group
				if (!us_edge || !new_us_edge) {
					cerr << "Node::SplitColoredMeshAtNode()- not all bad nodes, "
						<< " not all good nodes, and not one of each! "
						<< " I'm confused!\n";
					continue;
				}
	            
				if (us_edge->IsBorder()) {    // is boundary- just move it
					if (debug) cerr << "  is a boundary face- just move bad edge\n";
	                
					// move edge to point to new_us
					if(us_edge->endpoints[0] == us)
						us_edge->endpoints[0] = new_us;
					else if(us_edge->endpoints[1] == us)
						us_edge->endpoints[1] = new_us;

					new_us->edges.push_back(us_edge);

					int edgeIndex = -1;
					for(int i = 0; i<us->edges.size(); i++)
					{
						if(edges[i] == us_edge)
						{
							edgeIndex = i;
							break;
						}
					}

					if(edgeIndex != -1)
					{
						edges[edgeIndex] = edges[edges.size() - 1];
						edges.pop_back();
					}
	                
					// fix up face ptrs
					for(int i = 0; i<3; i++)
					{
						if(cur_face->points[i] == us)
							cur_face->points[i] = new_us;
					}

					new_us->faces.push_back(cur_face);
					int faceIndex = -1;
					for(int i = 0; i<faces.size(); i++)
					{
						if(faces[i] == cur_face)
						{
							faceIndex = i;
							break;
						}
					}

					if(faceIndex != -1)
					{
						faces[faceIndex] = faces[faces.size() - 1];
						faces.pop_back();
					}
				}
				else {    // not a boundary, must create new edge
					if (debug) cerr << "  not a boundary face- creating new edge\n";
	                
					// get a ptr to the other node
					Point* b;
					if(us_edge->endpoints[0] == us)
						b = us_edge->endpoints[1];
					else
						b = us_edge->endpoints[0];
	                
					// Not a boundary, so we keep the edge to us, but make a
					// new one to new_us
					//b->deleteEdgeLink(us_edge); 

					Edge* new_edge = db.AddEdge(b, new_us);
					// Note: the above does a "b->addEdgeLink(new_edge);" 
					// and a "new_us->addEdgeLink(new_edge);", too
	                
					// fix face links
					for(int i = 0; i<3; i++)
					{
						if(cur_face->points[i] == us)
							cur_face->points[i] = new_us;
					}

					new_us->faces.push_back(cur_face);
					int faceIndex = -1;
					for(int i = 0; i<faces.size(); i++)
					{
						if(faces[i] == cur_face)
						{
							faceIndex = i;
							break;
						}
					}

					if(faceIndex != -1)
					{
						faces[faceIndex] = faces[faces.size() - 1];
						faces.pop_back();
					}
	                
					// fix link for edge
					new_edge->faces[0] = cur_face;
					
					if(us_edge->faces[0] == cur_face)
						us_edge->faces[0] = NULL;
					else if (us_edge->faces[1] == cur_face)
						us_edge->faces[1] = NULL;

					//cur_face->replaceEdgeLink(us_edge, new_us_edge);
					for(int j = 0; j<3; j++)
					{
						if(cur_face->edges[j] == us_edge)
							cur_face->edges[j] = new_edge;
					}
				}
			} // for i
		}
	    
		return(new_us);
	}

	float GeoCuttableSurface::ShapeDatabase::CalcLengthSquared(Point * pt0, Point * pt1)
	{
		Vec3f coord0 = _owner->coord->getValue()->getCoord(pt0->index);
		Vec3f coord1 = _owner->coord->getValue()->getCoord(pt1->index);

		return coord0.x * coord1.x + coord0.y * coord1.y + coord0.z * coord1.z;
	}

	float GeoCuttableSurface::ShapeDatabase::CalcLengthSquared(const Vec3f & pt0, const Vec3f & pt1)
	{
		return pt0.x * pt1.x + pt0.y * pt1.y + pt0.z * pt1.z;
	}

	vector<int> GeoCuttableSurface::ShapeDatabase::GetIndices()
	{
		vector<int> rVal;
		set<Face *>::iterator i;
		for(i = faces.begin(); i != faces.end() ; i++)
		{
			Point * e0 = (*i)->edges[0]->endpoints[0];
			Point * e1 = (*i)->edges[0]->endpoints[1];
			Point * te2 = (*i)->edges[1]->endpoints[0];
			Point * te3 = (*i)->edges[1]->endpoints[1];
			Point * e2;
			if(te2 == e0 || te2 == e1)
				e2 = te3;
			else
				e2 = te2;

			Vec3f coe0 = _owner->coord->getValue()->getCoord(e0->index);
			Vec3f coe1 = _owner->coord->getValue()->getCoord(e1->index);
			Vec3f coe2 = _owner->coord->getValue()->getCoord(e2->index);

			Vec3f v0 = coe0 - coe1;
			Vec3f v1 = coe2 - coe1;
			if(v0.crossProduct(v1).z < 0)
			{
				Point * temp = e2;
				e2 = e1;
				e1 = temp;
			}

			rVal.push_back(e0->index);
			rVal.push_back(e1->index);
			rVal.push_back(e2->index);
		}

		return rVal;
	}

	/*
	int GeoCuttableSurface::ShapeDatabase::GetClosestPoint(const Vec3f& pt)
	{
		// This is a bad algorithm. It would be better to somehow organize the points
		// and use that information instead.

		X3DCoordinateNode * tcd = static_cast<X3DCoordinateNode *>(_owner->coord->getValue());
		Coordinate * coord = dynamic_cast<Coordinate *>(tcd);
		double min = 1000.0;
		int rVal = -1;
		for(int i = 0; i<points.size(); i++)
		{
			float tLen = CalcLengthSquared(pt, coord->point->getValue()[i]);
			if ( tLen < min)
			{
				min = tLen;
				rVal = i;
			}
		}

		return rVal;
	}

	int GeoCuttableSurface::ShapeDatabase::GetClosestFace(const Vec3f& pt)
	{
		//Point * theVertex = points[GetClosestPoint(pt)];
		//cout<<"Closest point: "<<GetClosestPoint(pt)<<endl;

		//TODO Inefficiency here!
		for(int i = 0; i<faces.size(); i++)
		{
			if(IsOnFace(pt, i))
			{
				return i;
			}
		}

		return -1;
	}

	
	bool GeoCuttableSurface::ShapeDatabase::IsOnFace(const Vec3f& pt, int face)
	{
		if(face == -1)
			return false;

		int pt0 = edges[faces[face]->edges[0]]->endpoints[0];
		int pt1 = edges[faces[face]->edges[0]]->endpoints[1];
		int pt2 = edges[faces[face]->edges[1]]->endpoints[1] != edges[faces[face]->edges[0]]->endpoints[0] ?
			edges[faces[face]->edges[1]]->endpoints[1] :
			edges[faces[face]->edges[1]]->endpoints[0];

		X3DCoordinateNode * tcd = static_cast<X3DCoordinateNode *>(_owner->coord->getValue());
		Coordinate * coord = dynamic_cast<Coordinate *>(tcd);

		Vec3f a = coord->point->getValue()[pt0];
		Vec3f b = coord->point->getValue()[pt1];
		Vec3f c = coord->point->getValue()[pt2];

		return SameSide(pt, a, b,c) && SameSide(pt, b, a,c) && SameSide(pt, c, a,b);
	}

	bool GeoCuttableSurface::ShapeDatabase::SameSide(const Vec3f & p0, const Vec3f & p1, const Vec3f & a, const Vec3f & b)
	{
		Vec3f cp0 = (b - a).crossProduct(p0 - a);
		Vec3f cp1 = (b - a).crossProduct(p1 - a);
		if(cp0.dotProduct(cp1) >= 0)
			return true;
		return false;
	}
	*/

	// subdivides a face in 2 or 3 faces in function of the position of the point
// in the triangle. If the point is outside, it returns NULL, otherwise it
// returns a pointer to the new node created due to the split and the faces
// that were created
// notice: this function assumes the point S is in the plane of the triangle.
GeoCuttableSurface::ShapeDatabase::Point* GeoCuttableSurface::ShapeDatabase::subdivideFace(GeoCuttableSurface::ShapeDatabase::Face *initial_p,
																						   GeoCuttableSurface::ShapeDatabase::Point * node_S,
																						   GeoCuttableSurface::ShapeDatabase & db,
																						   vector<Vec3f> & coords) 
{
    cerr << "FaceArray::subdivideFace(" << initial_p << ", " << node_S << ")\n";

    const double precision = 0.001; //Mike Oren: Changed from 0.001
    double dist, dot;
	GeoCuttableSurface::ShapeDatabase::Point *p[3];
	p[0] = initial_p->points[0];
    p[1] = initial_p->points[1];
    p[2] = initial_p->points[2];
	//Vec3f center = initial_p->getCenter();
	Vec3f center = (coords[p[0]->index] + coords[p[1]->index] + coords[p[2]->index]) / 3;
    
    Vec3f p_S[3];
    Vec3f p_p[3];
    Vec3f p_center[3];
    Vec3f cross1, cross2;
	GeoCuttableSurface::ShapeDatabase::Face *t1, *t2, *t3;

    for (int i = 0; i < 3; i++){
        p_S[i] = coords[node_S->index] - coords[p[i]->index];
        
        // ie, if S is too close to one of the vertices, we do not subdivide
		if (p_S[i].length() < precision) { 
            cerr << "too close to vertex (length = " << p_S[i].length() 
                << "<" << precision << "\n";
            return(NULL); 
        }
        
        p_p[i] = coords[p[(i + 1) % 3]->index] - coords[p[i]->index];
        p_center[i] = center - coords[p[i]->index];
		cross1 = p_S[i].crossProduct(p_p[i]);
        cross2 = p_center[i].crossProduct(p_p[i]);
		dot = cross1.dotProduct(cross2);
        
        if (dot < -precision) { 
            cerr << "dot < 0 (=" << dot << ")\n"; 
            return(NULL); 
        }
        
        dist = cross1.length() / p_p[i].length();
        
        //case when S in on an edge of the initial face so we subdivide in 2 faces
        if(dist <= precision) {
            cerr << " split into 2 faces\n";
            
            //should destroy the old sphere tree before mucking with subdivide
            // avoid coredumps from other objects colliding with this one
			// JDS: TODO These lines don't look necessary b/c we don't use their collision mechanism
			//			 Check this for sure, though.
            /*getObject()->getObjectArray()->ResetCollisions();
            getObject()->DestroySphereTree();*/
            
            // and delete the face from our list
            db.DeleteFace(initial_p, false);
            
			// Create 2 new faces
			t1 = db.AddFace(node_S, p[(i+1) % 3], p[(i+2)%3]);
			t2 = db.AddFace(node_S, p[(i+2) % 3],p[i]);
            
            cerr << "split_node = " << node_S << endl;
            cerr << "subtri t1: " << t1 << endl;
            cerr << "subtri t2: " << t2 << endl;
            
			// JDS: TODO These lines don't look necessary b/c we don't use their collision mechanism
			//			 Check this for sure, though.
            /*getObject()->CreateSphereTree();*/
            return(node_S);
        }
    }
    
    /* case when S inside the initial face so we subdivide in 3 faces */
    cerr << " split into 3 faces\n";
    
    //should destroy the old sphere tree before mucking with subdivide
    // avoid coredumps from other objects colliding with this one
	// JDS: TODO These lines don't look necessary b/c we don't use their collision mechanism
	//			 Check this for sure, though.
    /*getObject()->getObjectArray()->ResetCollisions();
    getObject()->DestroySphereTree();*/
    
    // delete it from our list
    db.DeleteFace(initial_p, false);
    
    // initialize our new faces and link in to new edges
	t1 = db.AddFace(node_S, p[1], p[2]);
	t2 = db.AddFace(node_S, p[2], p[0]);
	t3 = db.AddFace(node_S, p[0], p[1]);
    
    cerr << "split_node = " << node_S << endl;
    cerr << "subtri t1: " << t1 << endl;
    cerr << "subtri t2: " << t2 << endl;
    cerr << "subtri t3: " << t3 << endl;
    
	// JDS: TODO These lines don't look necessary b/c we don't use their collision mechanism
	//			 Check this for sure, though.
    /*getObject()->CreateSphereTree();*/
    return(node_S);
}

GeoCuttableSurface::ShapeDatabase::Point* GeoCuttableSurface::ShapeDatabase::subdivideFace(GeoCuttableSurface::ShapeDatabase::Face *initial_p,
																						   Vec3f S,
																						   GeoCuttableSurface::ShapeDatabase & db,
																						   vector<Vec3f> & coords) 
{
    cerr << "FaceArray::subdivideFace(" << initial_p << ", " << S << ")\n";
    
    // S becomes a new Node 
	coords.push_back(S);
	GeoCuttableSurface::ShapeDatabase::Point* node_S = new GeoCuttableSurface::ShapeDatabase::Point;
	node_S->index = coords.size() - 1;
	db.points.insert(node_S);
	cerr << " Created new node S of index " << coords.size() - 1 <<": "<< node_S << endl;
    
    // and call the other subdivide
	GeoCuttableSurface::ShapeDatabase::Point* ret_ptr = subdivideFace(initial_p, node_S, db, coords);
    
    // test if we were unsuccessful (if so, kill our created node)
	if (ret_ptr == NULL)
	{
		db.DeletePoint(node_S);
		coords.pop_back();
	}
    
    // and return their status/new_node
    return(ret_ptr);
}

	GeoCuttableSurface::ShapeDatabase::ShapeDatabase(GeoCuttableSurface * owner)
	{
		_owner = owner;
	}

	GeoCuttableSurface::ShapeDatabase::~ShapeDatabase()
	{
		Clear();
	}

	void GeoCuttableSurface::ShapeDatabase::LogMesh()
	{
		/*
		cout<<"*** Mesh log ***"<<endl;
		for(int i = 0; i<faces.size(); i++)
		{
			cout<<"Face "<<i<<":"<<endl;
			for(int j = 0; j<3; j++)
			{
				cout<<"\tEdge "<<faces[i]->edges[j]<<":"<<endl;
				cout<<"\t\tPoint "<<edges[faces[i]->edges[j]]->endpoints[0]<<endl;
				cout<<"\t\tPoint "<<edges[faces[i]->edges[j]]->endpoints[1]<<endl;
			}
		}
		cout<<"*** Mesh log complete ***"<<endl;
		*/
	}

	void GeoCuttableSurface::ShapeDatabase::Clear()
	{
		set<Point *>::iterator i;
		for(i = points.begin(); i != points.end(); i++)
		{
			delete (*i);
		}

		points.clear();

		set<Edge *>::iterator j;
		for(j = edges.begin(); j != edges.end(); j++)
		{
			delete (*j);
		}

		edges.clear();

		set<Face *>::iterator k;
		for(k = faces.begin(); k != faces.end(); k++)
		{
			delete (*k);
		}

		faces.clear();
	}

	void GeoCuttableSurface::DatabaseUpdater::update()
	{
		if(!theDB)
			return;

		cerr<<"Got to one"<<endl;

		X3DCoordinateNode * tcd = static_cast<SFCoordinateNode *>(routes_in[0])->getValue();
		Coordinate * coord = dynamic_cast<Coordinate *>(tcd);

		vector<int> indices = static_cast<MFInt32 *>(routes_in[1])->getValue();

		if(coord->point->getValue().size() != theDB->points.size() ||
			indices.size() != 3 * theDB->faces.size())
		{
			cout<<"Updating"<<endl;
			// We need to update everything
			// If neither of these are true, we assume everything's okay.
			// This could be false--if it's a problem, we might have to regenerate.
			theDB->Clear();
			vector<ShapeDatabase::Point *> tPointList;

			cerr<<"Cleared, etc."<<endl;

			for(int i = 0; i < coord->point->getValue().size(); i++)
			{
				ShapeDatabase::Point * newPoint = new ShapeDatabase::Point;
				newPoint->index = i;
				theDB->points.insert(theDB->points.begin(), newPoint);
				tPointList.push_back(newPoint);
			}

			cerr<<"tPointList filled out with "<<tPointList.size()<<endl;
			cerr<<"tPointList Data"<<tPointList[0]<<endl;

			for(int i = 0; i < indices.size() / 3; i++)
			{
				cout<<"i: "<<i<<endl;
				cout.flush();
				theDB->AddFace(tPointList[indices[3 * i + 0]],
							   tPointList[indices[3 * i + 1]],
							   tPointList[indices[3 * i + 2]]);
			}
			
			cout<<"Number of indices: "<<indices.size()<<endl;
			cout<<"Number of faces: "<<theDB->faces.size()<<endl;
			
			cout<<"********"<<endl;
			vector<int> newIndices = theDB->GetIndices();
			for(int i = 0; i< indices.size() && i < 25; i++)
			{
				cout<<indices[i]<<"\t\t"<<newIndices[i]<<endl;
			}
		}
	}